/*
 * main.cpp
 *
 *  Created on: Oct 12, 2012
 *      Author: bouchier
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "qegpioint.h"
#include "RCTest.h"
#include "Metro.h"
#include "CQEI2C.h"
#include "ControlledMotor.h"

#define DEFAULT_TEST 3

#define PRINT_RATE 5
#define PI 3.14159265359
#define WHEEL_CIRCUMFERENCE 2 * PI * 2

#define RF_MOTOR 1
#define RF_STEERING 2
#define LF_MOTOR 13
#define MOT15 13

// Motor numbers, in order of encoder wiring
#define LM_STEER 7
#define LM_DRIVE 2
#define LB_DRIVE 14
#define LB_STEER 4
#define LF_DRIVE 15
#define LF_STEER 3
#define RM_STEER 6
#define RB_DRIVE 13
#define RB_STEER 5
#define RM_DRIVE 12
#define RF_STEER 1
#define RF_DRIVE 16

// PID parameters for motors 1 - 16 (on servo ports & H-bridge for both servo & motor use)
#define DRIVE_HMOTOR_KP 15.0
#define DRIVE_HMOTOR_KI 2.0
#define DRIVE_HMOTOR_KD 1.0
#define DRIVE_SMOTOR_KP 10.0
#define DRIVE_SMOTOR_KI 1.0
#define DRIVE_SMOTOR_KD 0.0
#define SERVO_SMOTOR_KP 3.5
#define SERVO_SMOTOR_KI 2.0
#define SERVO_SMOTOR_KD 2.0
#define SERVO_HMOTOR_KP 10.0
#define SERVO_HMOTOR_KI 1.0
#define SERVO_HMOTOR_KD 0.0


CQEI2C i2c = CQEI2C();		// instantiate the I2C driver
CQEGpioInt &gpio = CQEGpioInt::GetRef();

struct RCChannel *dirRcc;
struct RCChannel *speedRcc;
struct RCChannel *estopRcc;

void fatal( char *msg, int motNum)
{
	printf("ERROR %s motor %d\n", msg, motNum);
	exit(0);
}

#define TEST1_SPEED 10
void test1(int speed = TEST1_SPEED)			// default to TEST1_SPEED
{
	int runtime = 100;		// # of 50 msec frames
	int printRate = PRINT_RATE;

	// instantiate the controlled motors.
	ControlledMotor lmDrive = ControlledMotor(LM_DRIVE, i2c, true, WHEEL_CIRCUMFERENCE);
	ControlledMotor lmSteer = ControlledMotor(LM_STEER, i2c, true, true, 136);

	// initialize the motors with their encoders & anything else. Order must match the I2C chain order
	if (!lmSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal((char *)"initializing", LM_STEER);
	if (!lmDrive.init(DRIVE_SMOTOR_KP, DRIVE_SMOTOR_KI, DRIVE_SMOTOR_KD)) fatal((char *)"initializing", LM_DRIVE);

	// zero the servo
	lmSteer.driveToMechStop();
	sleep(1);
	lmSteer.driveToStraight();
	for (int i=0; i<80; i++) {
		lmSteer.updateMotor();
		usleep(50000);
	}
	lmSteer.zeroEncoder();

	// Set their speed
	lmDrive.setSpeed(speed);	// ips
	lmSteer.setDegrees(0);		// stay at center

	Metro metro = Metro(50);		// 50ms metronome

	while(runtime) {
		if (metro.check()) {		// if 50ms have passed & it's time to do the control loop
			lmSteer.updateMotor();		// run the PID loop
			lmDrive.updateMotor();		// run the PID loop
			if (printRate-- == 0) {
				printRate = PRINT_RATE;
				printf("LM DRIVE speed: %0.1f, power %0.1f, distance: %0.1f; LM SERVO degrees: %d, power: %0.1f\n",
						lmDrive.getSpeed(), lmDrive.getMotorPower(), lmDrive.getDistance(),
						lmSteer.getDegrees(), lmSteer.getMotorPower());
			}
			runtime--;
		}
	}
	return;
}

void test2()
{
	int target;
	int runtime = 200;		// # of 50 msec frames
	int printRate = PRINT_RATE;
	//int rv = 0;

	// instantiate the controlled motors.
	ControlledMotor lmSteer = ControlledMotor(LM_STEER, i2c, true, true, 136);

	// initialize the motors with their encoders & anything else. Order must match the I2C chain order
	if (!lmSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal((char *)"initializing", LM_STEER);

	// zero the servo
	lmSteer.driveToMechStop();
	sleep(1);
	lmSteer.driveToStraight();
	for (int i=0; i<80; i++) {
		lmSteer.updateMotor();
		usleep(50000);
	}
	lmSteer.zeroEncoder();

	printf("Servo initialized\n");
	sleep(5);

	// Set desired position
	target = -90;
	printf("Seeking to %d\n", target);
	lmSteer.setDegrees(target);	// ips

	Metro metro = Metro(50);		// 50ms metronome

	while(runtime) {
		if (runtime == 100) {				// move the servo a bit
			// Set desired position
			target = 45;
			printf("Seeking to %d\n", target);
			lmSteer.setDegrees(target);	// ips
			runtime--;
		}

		if (metro.check()) {		// if 50ms have passed & it's time to do the control loop
			lmSteer.updateMotor();		// run the PID loop
			if (printRate-- == 0) {
				printRate = PRINT_RATE;
				printf("RF degrees: %d, power %0.1f, \n",
						lmSteer.getDegrees(), lmSteer.getMotorPower());
			}
			runtime--;
		}
	}
	return;
}

float runPIDTest(ControlledMotor& servoMotor, float params[3])
{
	int target;
	int runtime = 1;
	int err;
	float errSum = 0;

	// set the PID parameters
	servoMotor.initPid(params[0], params[1], params[2]);

	// Set desired position
	target = 10;
	servoMotor.setDegrees(target);	// ips

	Metro metro = Metro(50);		// 50ms metronome

	while(runtime) {
		if (metro.check()) {		// if 50ms have passed & it's time to do the control loop
			if (runtime%40 == 0) {				// move the servo every 2 second

				// Set desired position to be alternately further away
				target = (0 - target) + ((target>0)?-20:20);
				/*printf("LM degrees: %d, power %0.1f, rqdegrees: %d, error %d, errSum %0.1f\n",
						servoMotor.getDegrees(), servoMotor.getMotorPower(), servoMotor.getRqDegrees(),
						err, errSum);*/
				if (target < -90 || target > 90) {
					break;
				}
				printf(".");
				servoMotor.setDegrees(target);
			}

			servoMotor.updateMotor();		// run the PID loop
			err = servoMotor.getDegrees() - servoMotor.getRqDegrees();
			errSum += (float)err * (float)err;
			if (errSum < 0.0) {
				printf("Internal error, errSum: %0.2f, err: %d ", errSum, err);
				exit(-1);
			}
			runtime++;
		}
	}
	return (errSum/runtime);
}

void test3(float tol = 0.5)
{
	float err, best_error;
	int n = 0;
	int i;
	float params[] = {1.0, 1.0, 0.0};
	float dparams[] = {1.0, 1.0, 1.0};

	// instantiate the controlled motors.
	ControlledMotor lmDrive = ControlledMotor(LM_DRIVE, i2c, true, WHEEL_CIRCUMFERENCE);
	ControlledMotor lmSteer = ControlledMotor(LM_STEER, i2c, true, true, 136);

	// initialize the motors with their encoders & anything else. Order must match the I2C chain order
	if (!lmSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal((char *)"initializing", LM_STEER);
	if (!lmDrive.init(DRIVE_SMOTOR_KP, DRIVE_SMOTOR_KI, DRIVE_SMOTOR_KD)) fatal((char *)"initializing", LM_DRIVE);

	// zero the servo
	lmSteer.driveToMechStop();
	sleep(1);
	lmSteer.driveToStraight();
	for (int i=0; i<80; i++) {
		lmSteer.updateMotor();
		usleep(50000);
	}
	lmSteer.zeroEncoder();

	printf("Servo initialized\n");
	sleep(1);


	best_error = runPIDTest(lmSteer, params);
	printf("best_error: %0.1f\n", err);

	while ((dparams[0] + dparams[1] + dparams[2]) > tol) {
		for (i=0; i<3; i++) {
			switch(i) {
			case 0: printf("p"); break;
			case 1: printf("i"); break;
			case 2: printf("d"); break;
			}
			params[i] += dparams[i];
			printf(" Trying [%0.2f %0.2f %0.2f] ", params[0], params[1], params[2]);
			err = runPIDTest(lmSteer, params);
			printf("-> err: %0.2f\n", err);
			if (err < best_error) {
				best_error = err;
				dparams[i] *= 1.1;
			} else {
				params[i] -= 2.0 * dparams[i];
				printf(" Trying [%0.2f %0.2f %0.2f] ", params[0], params[1], params[2]);
				err = runPIDTest(lmSteer, params);
				if (err < best_error) {
					best_error = err;
					dparams[i] *= 1.1;
				} else {
					params[i] += dparams[i];
					dparams[i] *= 0.9;
				}
			}
			n++;
			printf("\nTwiddle # %d: [%0.2f %0.2f %0.2f] -> best_error %0.2f, tolerance: %0.2f\n",
					n, params[0], params[1], params[2], best_error,
					(dparams[0] + dparams[1] + dparams[2]));
		}
	}
	return;
}

void test4()
{
}

void test5()
{
	int targetAngle;
	float rqSpeed;
	int printRate = PRINT_RATE;
	//int rv = 0;

	// instantiate the controlled motors.
	ControlledMotor lmDrive = ControlledMotor(LM_DRIVE, i2c, true, WHEEL_CIRCUMFERENCE);
	ControlledMotor lmSteer = ControlledMotor(LM_STEER, i2c, true, true, 136);

	// initialize the motors with their encoders & anything else. Order must match the I2C chain order
	if (!lmSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal((char *)"initializing", LM_STEER);
	if (!lmDrive.init(DRIVE_SMOTOR_KP, DRIVE_SMOTOR_KI, DRIVE_SMOTOR_KD)) fatal((char *)"initializing", LM_DRIVE);

	// zero the servo
	lmSteer.driveToMechStop();
	sleep(1);
	lmSteer.driveToStraight();
	for (int i=0; i<80; i++) {
		lmSteer.updateMotor();
		usleep(50000);
	}
	lmSteer.zeroEncoder();
	printf("Servo & motor initialized\n");

	// initialize the steering, speed & estop R/C structs & start interrupt monitoring
	dirRcc = initRCTest(gpio, 0);		// initialize RC channel on DIO 0
	speedRcc = initRCTest(gpio, 1);
	estopRcc = initRCTest(gpio, 2);

	Metro metro = Metro(50);
	while (1){
		if (metro.check()) {

			// convert R/C values to desired speed range -20 - +20 ips & angle range +/-90
			rqSpeed = (float)(speedRcc->pulseWidth-1460)/25.0;
			lmDrive.setSpeed(rqSpeed);
			targetAngle = (dirRcc->pulseWidth-1500)/5;
			lmSteer.setDegrees(targetAngle);

			// update the motors
			lmDrive.updateMotor();		// run the PID loop
			lmSteer.updateMotor();
			if (printRate-- == 0) {
				printRate = PRINT_RATE;
				printf("R/C Speed: %d, R/C Dir: %d, ", speedRcc->pulseWidth, dirRcc->pulseWidth);
				printf("LMDRIVE speed: %0.1f, power %0.1f, LMSTEER angle: %d, power %0.1f\n",
						lmDrive.getSpeed(), lmDrive.getMotorPower(),
						lmSteer.getDegrees(), lmSteer.getMotorPower());
			}
		}
	}

}
#if 0
void test6()
{
	ControlledMotor *rfMotor;

	/*
	 * instantiate the controlled motor & reset the encoder with fwd = ccw, seeklimit = ccw
	 */
	i2c.I2CInit();
	rfMotor = new ControlledMotor(RF_MOTOR, i2c, ControlledMotor::servo,
			true, true, 90);
	if (!rfMotor->init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal(RF_MOTOR);
	printf("Servo initialized with true true, 90 - ccw limit-seek, cw center seek expected\n");
	sleep(5);
	delete rfMotor;

	/*
	 * instantiate the controlled motor & reset the encoder with fwd = cw, seeklimit = ccw
	 */
	i2c.I2CInit();
	rfMotor = new ControlledMotor(RF_MOTOR, i2c, ControlledMotor::servo,
			false, true, 90);
	if (!rfMotor->init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal(RF_MOTOR);
	printf("Servo initialized with true true, 90 - cw limit-seek, cw center seek expected\n");
	sleep(5);
	delete rfMotor;

	/*
	 * instantiate the controlled motor & reset the encoder with fwd = ccw, seeklimit = cw
	 */
	i2c.I2CInit();
	rfMotor = new ControlledMotor(RF_MOTOR, i2c, ControlledMotor::servo,
			true, false, 90);
	if (!rfMotor->init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal(RF_MOTOR);
	printf("Servo initialized with true true, 90 - cw limit-seek, cw center seek expected\n");
	sleep(5);
	delete rfMotor;

	/*
	 * instantiate the controlled motor & reset the encoder with fwd = cw, seeklimit = cw
	 */
	i2c.I2CInit();
	rfMotor = new ControlledMotor(RF_MOTOR, i2c, ControlledMotor::servo,
			false, false, 90);
	if (!rfMotor->init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal(RF_MOTOR);
	printf("Servo initialized with true true, 90 - cw limit-seek, cw center seek expected\n");
	sleep(5);
	delete rfMotor;

	return;
}
#endif

void usage()
{
	printf("Usage: ControlledMotor testNum\n");
	printf("testNum can be:\n");
	printf("1: run motor at constant speed for 10 sec\n");
	printf("2: seek one servo to -90 then 45 degrees\n");
	printf("3: Tune servo PID\n");
	printf("4: unused\n");
	printf("5: control a servo & a motor with R/C\n");
}

int main(int argc, char **argv)
{
	int test = DEFAULT_TEST;	// which test to run
	int argval;		// value to pass into test

	if (argc == 2)
		test = atoi(argv[1]);
	if (argc == 3)
		argval = atoi(argv[2]);
	printf("Running test %d\n", test);

	switch (test) {
	case 1:
		if (argc == 3)
			test1(argval);
		else
			test1();
		 break;
	case 2: test2(); break;
	case 3: test3(); break;
	case 4: test4(); break;
	case 5: test5(); break;
	//case 6: test6(); break;
	default:
		printf("Invalid option\n");
		usage();
	}
}
